package com.hitqz.robot.driver.netty.handler;

import cn.hutool.core.util.ByteUtil;
import cn.hutool.extra.spring.SpringUtil;
import com.hitqz.robot.api.common.dto.warning.AddWarningDto;
import com.hitqz.robot.driver.dto.KcNavStatus;
import com.hitqz.robot.driver.dto.KcRobotRunningStatus;
import com.hitqz.robot.driver.netty.protocol.message.KcMessage;
import com.hitqz.robot.driver.util.*;
import io.netty.channel.ChannelHandler;
import io.netty.channel.ChannelHandlerContext;
import io.netty.channel.SimpleChannelInboundHandler;
import lombok.extern.slf4j.Slf4j;

import java.nio.ByteOrder;
import java.nio.charset.StandardCharsets;
import java.util.Arrays;
import java.util.Objects;
import java.util.Optional;

/**
 * @author xupkun
 * @date 2023/8/4
 */
@Slf4j
@ChannelHandler.Sharable
public class KcMessageHandler extends SimpleChannelInboundHandler<KcMessage> {


    @Override
    protected void channelRead0(ChannelHandlerContext channelHandlerContext, KcMessage message) throws Exception {
        // 处理执行码
        this.checkExecuteCode(message);
        // 处理命令应答报文
        int cmd = message.getCmd()&0xFF;
        byte[] data = message.getData();
        if (data.length <= 0) {
            return;
        }
        switch (cmd) {
            case 0x00:
                //写变量
                break;
            case 0x01:
                //读变量
                //x00 U8[16] 变量名，采用 ascii 编码
                //0x10 U8[256] 变量值
                byte[] nameBytes = Arrays.copyOfRange(data, 0x00, 0x10);
                String varName = new String(nameBytes, StandardCharsets.US_ASCII).trim();
                //变量值的大小取决于该变量的变量类型
                byte[] valueBytes = Arrays.copyOfRange(data, 0x10, 0x10 + 256);
                DataTypeEnum dataTypeEnum = DataTypeEnum.VAR_NAME_ENUM_MAP.get(varName);
                String value = "";
                if (dataTypeEnum == null) {
                    log.warn("[kc]变量：{},not dataTypeEnum",varName);
                }else if (dataTypeEnum==DataTypeEnum.UDINT){
                    value = String.valueOf(ByteUtil.bytesToInt(valueBytes, ByteOrder.LITTLE_ENDIAN));
                }else if (dataTypeEnum==DataTypeEnum.REAL){
                    value = String.valueOf(ByteUtil.bytesToFloat(valueBytes, ByteOrder.LITTLE_ENDIAN));
                }else if (dataTypeEnum==DataTypeEnum.INT){
                    value = String.valueOf(ByteUtil.bytesToShort(valueBytes, ByteOrder.LITTLE_ENDIAN));
                }else if (dataTypeEnum==DataTypeEnum.BOOL){
                    value = String.valueOf(valueBytes[0]);
                }
                KcCacheUtil.setVarCache(varName,value);
                //log.info("varName: {} --- value {}--{} ", varName, value, KcCacheUtil.getVarCache(varName));
                break;
            case 0x11:
                 //手自动切换
                 break;
            case 0x14:
                 //机器人手动定位
                 break;
            case 0x16:
                 //导航控制
                 break;
            case 0x17: {
                //查询机器人运行状态
                KcRobotRunningStatus lastRunningStatus = Optional.ofNullable((KcRobotRunningStatus) KcCacheUtil.get(KcCacheUtil.KEY_RUNNING_STATUS)).orElse(new KcRobotRunningStatus());
                double bodyTemperature = ByteUtil.bytesToDouble(Arrays.copyOfRange(data, 0x00, 0x08), ByteOrder.LITTLE_ENDIAN);//本体温度
                double positionX = ByteUtil.bytesToDouble(Arrays.copyOfRange(data, 0x08, 0x10), ByteOrder.LITTLE_ENDIAN);//x坐标
                double positionY = ByteUtil.bytesToDouble(Arrays.copyOfRange(data, 0x10, 0x18), ByteOrder.LITTLE_ENDIAN);//y坐标
                double orientationAngle = ByteUtil.bytesToDouble(Arrays.copyOfRange(data, 0x18, 0x20), ByteOrder.LITTLE_ENDIAN);//转向
                double batteryPower = ByteUtil.bytesToDouble(Arrays.copyOfRange(data, 0x20, 0x28), ByteOrder.LITTLE_ENDIAN);//电量 0~1
                int isBlocked = data[0x28] & 0xff;//是否被阻挡 0-否 1-是
                if(!Objects.equals(lastRunningStatus.getIsBlocked(),isBlocked) && isBlocked==1) {
                    log.info("[kc]robot is blocked...");
                }
                int isCharging = data[0x29] & 0xff;//是否在充电 0-否 1-是
                int runMode = data[0x2A] & 0xff;//运行模式 0-手动 1-自动
                int mapLoadStatus = data[0x2B] & 0xff;//地图载入状态 0-成功 1-失败 2-未载入 3-正在载入
                int targetPointId = ByteUtil.bytesToInt(Arrays.copyOfRange(data, 0x2C, 0x30), ByteOrder.LITTLE_ENDIAN);//目标点id
                double forwardSpeed = ByteUtil.bytesToDouble(Arrays.copyOfRange(data, 0x30, 0x38), ByteOrder.LITTLE_ENDIAN);//前进速度
                double turnSpeed = ByteUtil.bytesToDouble(Arrays.copyOfRange(data, 0x38, 0x40), ByteOrder.LITTLE_ENDIAN);//转弯速度
                double batteryVoltage = ByteUtil.bytesToDouble(Arrays.copyOfRange(data, 0x40, 0x48), ByteOrder.LITTLE_ENDIAN);//电池电压，正充电，负放电
                double current = ByteUtil.bytesToDouble(Arrays.copyOfRange(data, 0x48, 0x50), ByteOrder.LITTLE_ENDIAN);//电池电流，正充电，负放电
                int taskStatus = data[0x50] & 0xff;//当前任务状态 0-无前往导航点的任务 1-等待 2-正在前往导航点 3-暂停 4-完成 5-失败 6-退出 7-等待开/关门
                int reserved1 = data[0x51] & 0xff; //保留位1
                int mapVersion = ByteUtil.bytesToShort(Arrays.copyOfRange(data, 0x52, 0x54), ByteOrder.LITTLE_ENDIAN); //地图版本号
                byte[] reserved2 = Arrays.copyOfRange(data, 0x54, 0x58); // 保留位2
                double totalMileage = ByteUtil.bytesToDouble(Arrays.copyOfRange(data, 0x58, 0x60), ByteOrder.LITTLE_ENDIAN); //累计里程 m
                double currentRunTime = ByteUtil.bytesToDouble(Arrays.copyOfRange(data, 0x60, 0x68), ByteOrder.LITTLE_ENDIAN); //本次运行时间 ms
                double totalRunTime = ByteUtil.bytesToDouble(Arrays.copyOfRange(data, 0x68, 0x70), ByteOrder.LITTLE_ENDIAN);//累计运行时间 ms
                int robotPositionStatus = data[0x70] & 0xff;//定位状态 0-失败 1-成功 2-定位中 3-定位完成
                byte[] reserved3 = Arrays.copyOfRange(data, 0x71, 0x74);//保留位3
                int mapCount = ByteUtil.bytesToInt(Arrays.copyOfRange(data, 0x74, 0x78), ByteOrder.LITTLE_ENDIAN);//地图数量
                String currentMapName = new String(Arrays.copyOfRange(data, 0x78, 0xb8), StandardCharsets.UTF_8).trim();//当前地图名称
                float confidence = ByteUtil.bytesToFloat(Arrays.copyOfRange(data, 0xb8, 0xbc), ByteOrder.LITTLE_ENDIAN);//置信度0~1
                byte[] reserved4 = Arrays.copyOfRange(data, 0xbc, 0xbc + 4);//保留位4
                KcRobotRunningStatus kcRobotRunningStatus = new KcRobotRunningStatus(bodyTemperature, positionX, positionY, orientationAngle, batteryPower, isBlocked,
                        isCharging, runMode, mapLoadStatus, targetPointId, forwardSpeed, turnSpeed, batteryVoltage,
                        current, taskStatus, reserved1, mapVersion, reserved2, totalMileage, currentRunTime,
                        totalRunTime, robotPositionStatus, reserved3, mapCount, currentMapName, confidence, reserved4,1);
                log.debug("[kc]robot running status:{}", kcRobotRunningStatus);
                KcCacheUtil.put(KcCacheUtil.KEY_RUNNING_STATUS, kcRobotRunningStatus);
                break;
            }
            case 0xAF: {
                 //混合导航状态
                //int errorMessageNumber =  data[0] & 0xff;//异常事件状态信息数量
                //int actionStatusNumber = data[1] & 0xff; // 动作状态数量
                //byte[] reserved1 = Arrays.copyOfRange(data, 2, 4); // 预留
                int lastPoint = ByteUtil.bytesToInt(Arrays.copyOfRange(data, 16, 20));
                int lasLine = ByteUtil.bytesToInt(Arrays.copyOfRange(data, 20, 24));
                if (lastPoint == 0 && lasLine >=0) {
                    // 二维码上面
                    KcCacheUtil.put(KcCacheUtil.KEY_CURRENT_STATION, lasLine);
                } else {
                    KcCacheUtil.put(KcCacheUtil.KEY_CURRENT_STATION, lastPoint);
                }
                int robotStatus = data[49] & 0xff;
                KcCacheUtil.put(KcCacheUtil.KEY_ROBOT_RUNNING_STATUS, robotStatus);
                break;
            }
            case 0x1D: {
                //查询机器人导航状态
                KcNavStatus lastNavStatus =Optional.ofNullable((KcNavStatus) KcCacheUtil.get(KcCacheUtil.KEY_NAV_STATUS)).orElse(new KcNavStatus());
                int status = data[0] & 0xff;//导航状态 0-无到导航点任务 1-等待 2-正在前往导航点 3-暂停 4-完成 5失败 6-退出 7-等待开/关门
                byte[] reserved1 = Arrays.copyOfRange(data, 0x01, 0x04);//保留位1
                short targetPointId = ByteUtil.bytesToShort(Arrays.copyOfRange(data, 0x04, 0x06), ByteOrder.LITTLE_ENDIAN);//目标点id
                byte[] reserved2 = Arrays.copyOfRange(data, 0x06, 0x08);//保留位2
                short[] passedPathPointIds = new short[126];//已经经过的路径点 ID 0 表示无效路径点
                for (int i = 0; i < 126; i++) {
                    passedPathPointIds[i] = ByteUtil.bytesToShort(Arrays.copyOfRange(data, 0x08 + i * 2, 0x08 + (i + 1) * 2), ByteOrder.LITTLE_ENDIAN);
                }
                short[] unPassedPathPointIds = new short[126];//未经过的路径点 ID 0 表示无效路径点
                for (int i = 0; i < 126; i++) {
                    unPassedPathPointIds[i] = ByteUtil.bytesToShort(Arrays.copyOfRange(data, 0x104 + i * 2, 0x104 + (i + 1) * 2), ByteOrder.LITTLE_ENDIAN);
                }
                //log.info("[kc] robot go to pos:{},{}",targetPointId, KcNavStatusEnum.getDes(status));
                if (!Objects.equals(lastNavStatus.getStatus(), status)){
                    log.info("[kc] robot go to pos:{},{}",targetPointId, KcNavStatusEnum.getDes(status));
                }
                KcNavStatus kcNavStatus = new KcNavStatus(status, reserved1, targetPointId, reserved2, passedPathPointIds, unPassedPathPointIds);
                log.debug("[kc]robot nav status:{}", kcNavStatus);
                KcCacheUtil.put(KcCacheUtil.KEY_NAV_STATUS,kcNavStatus);
                break;
            }
            case 0x1F:
                 //确认初始位置
                 break;
            default:
                log.warn("[kc]unknown cmd:{},seq:{},data:{}", message.getCmdHexStr(), message.getSequence(), data);
                break;
        }
        //释放线程
        KcToolkit.release(message);
    }

    @Override
    public void exceptionCaught(ChannelHandlerContext ctx, Throwable cause) {
        log.error("[kc]handler Exception occurred: ", cause);
        ctx.close();
    }



    private boolean checkExecuteCode(KcMessage dto) {
        byte executeCode = dto.getExecuteCode();
        ExecuteCodeEnum executeCodeEnum = ExecuteCodeEnum.codeMap.get(executeCode);
        if (executeCodeEnum == null) {
            //log.warn("[kc]seq:{},cmd:{},unknown executeCode：{}",dto.getSequence(),dto.getCmdHexStr(), executeCode);
            return false;
        }
        if (executeCodeEnum == ExecuteCodeEnum.SUCCESS) {
            //log.debug("[kc]seq:{},cmd:{},{}", dto.getSequence(),dto.getCmdHexStr(), executeCodeEnum.getDesc());
            return true;
        }
        //log.error("[kc]seq:{},cmd:{},desc:{}", dto.getSequence(), dto.getCmdHexStr(), executeCodeEnum.getDesc());
        if (executeCodeEnum == ExecuteCodeEnum.FAILED && dto.getCmd() == (byte) 0x01){
            byte[] data = dto.getData();
            //x00 U8[16] 变量名，采用 ascii 编码
            byte[] nameBytes = Arrays.copyOfRange(data, 0x00, 0x10);
            String varName = new String(nameBytes, StandardCharsets.US_ASCII).trim();
            DataTypeEnum.VAR_NAME_SET.remove(varName);
            DataTypeEnum.VAR_NAME_ENUM_MAP.remove(varName);
            log.info("[kc]变量:{},获取失败判断为当前控制器无这个变量,移除后续查询",varName);
            return false;
        }
        //todo 发布报警事件
        //publishWarningEvent(HexUtil.toHex(executeCodeEnum.getCode()), executeCodeEnum.getDesc());
        return false;
    }

    private void publishWarningEvent(String executeCode,String content) {
        AddWarningDto addWarningDto = new AddWarningDto();
        addWarningDto.setCode(executeCode);
        addWarningDto.setTitle("控制器报警");
        addWarningDto.setContent(content);
        addWarningDto.setSuggestion("无");
        addWarningDto.setLevel(1);
        SpringUtil.getApplicationContext().publishEvent(addWarningDto);
    }



}
